#include <mc_map/bev_map.h>

BevMap::BevMap(){
    hasInit_ = false;
}

// 初始化参数
void BevMap::Init(float width, float height, float resolution, string frame_id){
    resolution_ = resolution;
    widthMeter_ = width;
    heightMeter_ = height;
    frameId_ = frame_id;
    width_ = int(widthMeter_ / resolution_); 
    height_ = int(heightMeter_ / resolution_);
    hasInit_ = true;
}

// // 设置雷达点云数据  相对于base_link
// void BevMap::setPointCloudData(sensor_msgs::PointCloud2::Ptr cloud_ptr){
//     if(!cloud_ptr) return;
//     if(!hasInit_) return; 

//     cvMap_ = cv::Mat(height_, width_, CV_8UC1, 255);

//     sensor_data::PointCloudXYZPtr pcl_cloud_ptr(new sensor_data::PointCloudXYZ);
//     pcl::fromROSMsg(*pointcloud2_ptr, *pcl_cloud_ptr);

//     sensor_data::PointCloudXYZPtr pointcloudxyz_ptr = msg_convert::convertToPointCloudXYZ(cloud_ptr);
//     // cout << "[setPointCloudData] size: " << pointcloudxyz_ptr->points.size() << endl;
//     float center_x = widthMeter_ / 2.0;
//     float center_y = heightMeter_ / 2.0;
//     for(int i=0; i<pointcloudxyz_ptr->points.size(); i++){
//         pcl::PointXYZ point = pointcloudxyz_ptr->points[i];
//         // 左下角原点为（-center_x, -center_y）
//         int mx = std::round((point.x + center_x) / resolution_);
//         int my = std::round((point.y + center_y) / resolution_);
//         my = height_ - my;      //转为opencv坐标系
//         // cout << "x: " << point.x << ", y:" << point.y
//         //     << ", mx: " << mx << ", my: " << my
//         //     << ", center_x: " << center_x << ", center_y: " << center_y
//         //     << endl;
//         if(mx<0 || mx >= width_) continue;
//         if(my<0 || my >= height_) continue;
//         cvMap_.at<uchar>(my, mx) = 0;
//     }

//     // DEBUG
//     // cv::imwrite("/home/xxty/trobot/trobot_ros/trobot_ws/bev_map.png", cvMap_);
// }

bool BevMap::getBevMap(cv::Mat& cv_map){
    return getMap(cv_map);
}

bool BevMap::getBevMap(nav_msgs::OccupancyGrid::Ptr& map_ptr, string frame_id){
    if(cvMap_.empty()) return false;
    map_ptr->data.resize(width_ * height_);
    // cout << "-----------------------------------------" << endl;
    for(int w = 0; w < cvMap_.cols; w++) {
        for(int h = 0; h < cvMap_.rows; h++) {
            if(cvMap_.at<uchar>(h, w) == 0){
                int flipped_h = cvMap_.rows - 1 - h;   //反转y轴坐标 (ros中原点在左下角，opencv中原点在左上角)
                size_t idx = flipped_h * width_ + w;
                if(idx <0 || idx > map_ptr->data.size()) continue;
                map_ptr->data[idx] = 100;
            }       
        }
    }
    map_ptr->info.resolution = resolution_;
    map_ptr->info.width = cvMap_.cols;
    map_ptr->info.height = cvMap_.rows;
    map_ptr->info.origin.position.x = -widthMeter_ / 2.0;
    map_ptr->info.origin.position.y = -heightMeter_ / 2.0;
    map_ptr->header.stamp = ros::Time::now();
    map_ptr->header.frame_id = frame_id;
    return true;
}